import com.google.gson.Gson;
import models.*;
import retrofit2.Retrofit;
import retrofit2.converter.gson.GsonConverterFactory;

import java.net.DatagramPacket;
import java.net.DatagramSocket;
import java.net.InetAddress;

public class Main {
    public static void main(String[] args) {
        /*
         * 整个程序包含以下几个过程。
         * 获取局域网机器人广播，然后获取机器人Token。
         * 获取机器人当前基本信息，获取机器人当前导航坐标，然后发布导航任务，机器人移动到前方0.5m处，并等待任务完成。
         * 使用速度控制API，让机器人原地旋转1秒。
         * 重置机器人遥控速度参数
         * 这几个API分别包含了几种API的请求方式，GET, POST, PUT, DELETE。其他API调用和这几个例子一样。
         */

        // 获取机器人UDP广播
        Gson gson = new Gson();
        String robotIP;
        try
        {
            DatagramSocket socket = new DatagramSocket(22002, InetAddress.getByName("0.0.0.0"));
            DatagramPacket packet = new DatagramPacket(new byte[1024],  1024);
            socket.setSoTimeout(60 * 1000);
            socket.receive(packet);
            byte[] receivedData = new byte[packet.getLength()];
            byte[] packetData = packet.getData();
            System.arraycopy(packetData, 0, receivedData, 0, packet.getLength());
            String data = new String(receivedData);
            System.out.println(data);
            ServerInfo info = gson.fromJson(data, ServerInfo.class);
            System.out.printf("Received UDP broadcast: %s%n", info.id);
            robotIP = packet.getAddress().getHostAddress();
        }catch (Exception e){
            e.printStackTrace();
            return;
        }

        Retrofit retrofit = new Retrofit.Builder()
                .baseUrl(String.format("http://%s:3546/", robotIP))
                .addConverterFactory(GsonConverterFactory.create()).build();
        GalileoAPI galileoAPI = retrofit.create(GalileoAPI.class);
        // 获取机器人token，采用默认的用户名密码
        TokenInfo tokenInfo;
        try
        {
            tokenInfo = galileoAPI.getToken("admin", "admin").execute().body();
        }catch (Exception ignore){
            System.out.println("Get robot token failed");
            return;
        }
        if(tokenInfo == null)
            return;
        // 获取机器人基本信息
        System.out.println("Start get robot info");
        RobotInfo robotInfo;
        try
        {
            robotInfo = galileoAPI.getRobotInfo(tokenInfo.token).execute().body();
            if(robotInfo == null)
                return;
            System.out.printf("Current battery: %d%n", robotInfo.battery);
        }catch (Exception ignore){
            System.out.println("Get robot info failed");
        }
        try {
            Thread.sleep(3000);
        } catch (InterruptedException e) {
            e.printStackTrace();
        }
        // 执行前先开启机器人导航
        // 获取当前坐标
        System.out.println("Start get robot pose");
        Pose currentPose;
        Task taskInfo;
        try
        {
            currentPose = galileoAPI.getRobotPose(tokenInfo.token).execute().body();
            if(currentPose == null)
            {
                System.out.println("Get robot pose failed");
                return;
            }
            PoseReq poseReq = new PoseReq();
            poseReq.x = currentPose.x + 0.5 * Math.cos(currentPose.angle);
            poseReq.y = currentPose.y + 0.5 * Math.sin(currentPose.angle);
            poseReq.theta = currentPose.angle;
            taskInfo = galileoAPI.startNavTask(tokenInfo.token, poseReq).execute().body();
            if(taskInfo == null)
            {
                System.out.println("Send navigation task failed");
                return;
            }
            // 通过轮询等待任务执行完成
            while (true)
            {
                Task currentTask = galileoAPI.getTask(taskInfo.id, tokenInfo.token).execute().body();
                if(currentTask == null)
                {
                    System.out.println("Get current task status failed");
                    return;
                }
                if(currentTask.state.equals("COMPLETE")  || currentTask.state.equals("CANCELLED")
                        || currentTask.state.equals("ERROR"))
                    break;
                System.out.println("Waiting for task complete...");
                Thread.sleep(1000);
            }
        }catch (Exception ignore){
            System.out.println("Get robot pose failed");
            return;
        }

        // 让机器人原地旋转1s
        try
        {
            SetSpeedReq setSpeedReq = new SetSpeedReq();
            setSpeedReq.speed_x = 0;
            setSpeedReq.speed_y = 0;
            setSpeedReq.speed_angle = 1.0f;
            SimpleResponse response = galileoAPI.setSpeed(tokenInfo.token, setSpeedReq).execute().body();
            if(response == null)
            {
                System.out.println("Set robot speed failed");
                return;
            }
            Thread.sleep(1000);
            setSpeedReq.speed_angle = 0;
            SimpleResponse response1 = galileoAPI.setSpeed(tokenInfo.token, setSpeedReq).execute().body();
            if(response1 == null)
            {
                System.out.println("Set robot speed failed");
            }
        }catch (Exception ignore)
        {
            System.out.println();
        }

        // 重置机器人最大遥控速度 max_control_speed
        try
        {
            SystemConfig response = galileoAPI.resetSystemConfig(tokenInfo.token, "max_control_speed").execute().body();
            if(response == null)
            {
                System.out.println("Reset max control speed success");
            }
        }catch (Exception ignore)
        {
            System.out.println("Reset max control speed success");
        }
    }
}
